use reqwest;
use std::{net::UdpSocket};
use serde_json;
use demo::models::{ServerInfo, TokenResponse, SystemStatus, Pose,
     TargetPose, TaskInfo, TaskState, SetSpeedReq};

/**
整个程序包含以下几个过程。
获取局域网机器人广播，然后获取机器人Token。
获取机器人当前基本信息，获取机器人当前导航坐标，然后发布导航任务，机器人移动到前方0.5m处，并等待任务完成。
使用速度控制API，让机器人原地旋转1秒。
重置机器人遥控速度参数
这几个API分别包含了几种API的请求方式，GET, POST, PUT, DELETE。其他API调用和这几个例子一样。
*/

#[tokio::main]
async fn main() {
    // 获取机器人UDP广播
    let server_info:ServerInfo;
    let robot_ip:String;
    {
        let socket = UdpSocket::bind("0.0.0.0:22002").unwrap();
        socket.set_read_timeout(Some(std::time::Duration::from_secs(60))).unwrap();
        let mut buf = [0;1024];
        let (amt, src) = socket.recv_from(&mut buf).unwrap();
        let buf = Vec::from(&mut buf[..amt]);
        server_info = serde_json::from_slice(&* buf).unwrap();
        robot_ip = src.ip().to_string();
    }
    println!("{}", serde_json::to_string_pretty(&server_info).unwrap());
    let api_url = format!("http://{}:3546/api/v1", &robot_ip);
    // 获取机器人token，采用默认的用户名密码
    let client = reqwest::Client::new();
    let res = client.get(format!("{}/token?username=admin&password=admin", api_url))
        .send().await.unwrap().text().await.unwrap();
    let token: TokenResponse = serde_json::from_str(&* res).unwrap();
    println!("Get robot token success");
    println!("Start get robot info");
    // 获取机器人基本信息
    let res = client.get(format!("{}/system/info?token={}", &api_url, &token.token))
        .send().await.unwrap().text().await.unwrap();
    let system_info:SystemStatus = serde_json::from_str(&* res).unwrap();
    // 打印当前电压
    println!("Current battery: {}", system_info.battery);
    std::thread::sleep(std::time::Duration::from_secs(3));
    // 执行前先开启机器人导航
    // 获取当前坐标
    println!("Start get robot pose");
    let res = client.get(format!("{}/navigation/pose?token={}", api_url, token.token))
        .send().await.unwrap().text().await.unwrap();
    let current_pose:Pose = serde_json::from_str(&* res).unwrap();
    println!("Current pose: x {} y {} theta {}", current_pose.x, current_pose.y, current_pose.angle);
    std::thread::sleep(std::time::Duration::from_secs(3));
    println!("Start move to target point");
    let x = current_pose.x + 0.5 * current_pose.angle.cos();
    let y = current_pose.y + 0.5 * current_pose.angle.sin();
    let pose = TargetPose {
        x: x,
        y: y,
        theta: current_pose.angle
    };
    let res = client.post(format!("{}/navigation/start_nav_task?token={}", api_url, token.token))
        .json(&pose)
        .send().await.unwrap().text().await.unwrap();
    let task_info: TaskInfo = serde_json::from_str(&* res).unwrap();
    // 通过轮询等待任务执行完成
    loop {
        let res = client.get(format!("{}/task?id={}&token={}", api_url, task_info.id, token.token))
            .send().await.unwrap().text().await.unwrap();
        let info:TaskInfo = serde_json::from_str(&* res).unwrap();
        match info.state {
            TaskState::ERROR => break,
            TaskState::CANCELLED => break,
            TaskState::COMPLETE => break,
            _ => {}
        };
        println!("Waiting for task to complete");
        std::thread::sleep(std::time::Duration::from_secs(1));
    }
    // 让机器人原地旋转1s
    let mut speed_req = SetSpeedReq {
        speed_x: 0.0,
        speed_y: 0.0,
        speed_angle: 1.0,
    };
    client.put(format!("{}/system/speed?token={}", api_url, token.token))
        .json(&speed_req)
        .send().await.unwrap();
    std::thread::sleep(std::time::Duration::from_secs(1));
    speed_req.speed_angle = 0.0;
    // 停止旋转
    client.put(format!("{}/system/speed?token={}", api_url, token.token))
        .json(&speed_req)
        .send().await.unwrap();
    // 重置机器人最大遥控速度 max_control_speed
    client.delete(format!("{}/system/config?token={}&key=max_control_speed", api_url, token.token))
        .send().await.unwrap();
    println!("Reset max control speed success");
}
